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Abstract 


The  manipulator  trajectory  tracking  control  problem  revolves  around  computing  the 
torques  to  be  applied  to  achieve  accurate  tracking.  While  this  problem  has  been 
extensively  studied  in  simulations,  the  real-time  results  have  been  lacking  in  the  robotics 
literature.  *tn  this  papery  we  present,  the  experimental  results  of  the  real-time  performance 
of  model-based  control  algorithms.  We  compare, the  computed- torque  control  scheme  with 
the  feedforward  dynamics  compensation  scheme.  The  feedforward  scheme  compensates 
for  the  manipulator  dynamics  in  the  feedforward  path  while  the  computed- torque  scheme 
uses  the  dynamics  in  the  feedback  loop  for  linearization  and  decoupling.  The  manipulator 
control  schemes  have  been  implemented  on  the  CMU  DD  Arm  II  with  a  sampling  period  of 
2  ms. 
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1.  Introduction 

The  manipulator  control  problem  revolves  around  the  computation  of  the  joint  torques 
required  to  track  the  desired  joint  position,  velocity  and  acceleration  trajectories.  This 
problem  has  been  studied  extensively  in  the  robotics  literature  and  many  schemes  have 
been  proposed  [3,  4,  5,  6,  14,  15].  While  many  simulation  results  have  been  presented,  the 
real-time  implementation  and  performance  evaluation  of  model-based  control  schemes  on 
actual  manipulators  has  not  been  performed.  The  main  reasons  for  this  are:  (1)  the  high 
gear  ratios  and  the  dominant  friction  effects  in  commercial  geared  manipulators  make 
them  unsuitable  for  real-time  performance  evaluation;  (2)  the  computational  requirements 
of  the  Newton-Euler  algorithm  are  still  beyond  the  reach  of  commercially  available 
microprocessors  [7]  for  high  sample-rate  control;  and  (3)  it  has  been  difficult  to  obtain  an 
accurate  model  because  research  in  this  area  has  been  lacking. 

One  of  the  goals  of  the  CMU  Direct-Drive  Arm  II  [13]  project  is  to  demonstrate  the  effect 
of  full  dynamics  compensation  on  the  real-time  trajectory  performance  tracking  of 
manipulators  by  overcoming  the  above  mentioned  difficulties.  To  overcome  the  hurdle 
posed  by  the  computational  requirements,  we  have  customized  the  Newton-Euler 
algorithm  and  achieved  a  computational  cycle  of  1.2  ms.  This  permits  us  to  implement 
control  algorithms  at  high  sampling  rates  of  up  to  830  Hz.  We  have  also  proposed 
numerical  [8]  and  symbolic  algorithms  [9,  12]  to  estimate  the  dynamics  parameters  of  a 
manipulator  We  have  implemented  our  algorithm  on  the  CMU  Direct-Drive  Arm  II  to 
obtain  an  accurate  dynamics  model. 

The  above  developments  have  allowed  us  the  opportunity  to  implement  and  evaluate  the 
real-time  performance  of  advanced  manipulator  control  schemes.  The  experimental  results 
of  the  real-time  implementation  and  evaluation  of  model-based  control  schemes  were 
presented  recently,  wherein  the  performance  of  the  computed- torque  and  the  independent 
joint  control  schemes  was  compared  [10].  Other  researchers  have  also  investigated  this 
problem  and  evaluated  the  real-time  performance  of  the  model-based  schemes  [11,  1]. 

In  this  paper,  we  compare  the  computed- torque  scheme  with  the  feedforward  dynamics 
compensation  scheme.  Both  the  schemes  use  the  full  dynamics  model  of  the  manipulator 
but  in  different  paths  of  the  control  loop:  the  feedforward  scheme  compensates  for  the 
manipulator  dynamics  in  the  feedforward  path  while  the  computed- torque  scheme  uses  the 
dynamics  in  the  feedback  loop  for  linearisation  and  decoupling.  The  control  schemes  have 
been  implemented  on  the  CMU  DD  Arm  II  with  a  sampling  period  of  2  ms. 

This  paper  is  organised  as  follows:  the  manipulator  control  schemes  that  have  been 
implemented  and  evaluated  are  presented  in  Section  2  The  design  of  the  controller  gain 
matrices  is  outlined  in  Section  3  In  Section  4,  we  delineate  and  analyze  the  results  of  our 
real-time  implementation.  And  finally,  in  Section  5,  we  draw  our  conclusions. 
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2.  Manipulator  Control  Technique* 

The  dynamics  of  a  manipulator  are  described  by  a  set  of  highly  nonlinear  and  coupled 
differential  equations.  The  complete  dynamic  model  of  an  N  degrees-of-freedom 
manipulator  is  described  by: 

r  =  D(0)0  +  h(0,0)  +  g(0)  (!) 

where  r  is  the  N  vector  of  the  actuating  torques;  D(0)  is  the  NX  N  position  dependent 
manipulator  inertia  matrix;  h(0,0)  is  the  N  vector  of  Coriolis  and  centrifugal  torques;  g(0) 
is  the  iV  vector  of  gravitational  torques;  and  0,  6  and  fl  are  N  vectors  of  the  joint 
accelerations,  velocities  and  positions,  respectively. 

We  have  implemented  and  compared  the  performance  of  the  computed- torque  and  the 
feedforward  compensation  control  schemes.  In  order  to  evaluate  the  effect  of 
approximating  the  position  dependent  inertia  matrix  D(0)  by  a  constant  diagonal  inertia 
matrix  J,  we  have  also  implemented  the  reduced  feedforward  compensation  scheme.  In  the 
sequel,  Kp  and  Kv  are  the  constant  and  diagonal  position  and  velocity  feedback  gain 
matrices,  repectively;  0  and  0d  are  the  measured  and  the  reference  joint  position  vectors, 
repectively;  and  the  symbol  "•  denotes  the  derivative  with  respect  to  time. 

Computed-Torque  Control  Scheme  (CT) 

This  scheme,  depicted  in  Figure  1,  utilizes  nonlinear  feedback  to  decouple  the 
manipulator.  The  control  torque  r  is  computed  by  the  inverse  dynamics  equation  in  (1), 
using  the  commanded  acceleration  instead  of  the  measured  acceleration  0,  as: 

r  =  t>(0){Kp(0d-0)  +  Kv(0d-0)  +  0J  (2) 

+  K(M)  4-  f(0) 

where  the  ank  indicates  that  the  estimated  values  of  the  dynamics  parameters  are  used  in 
the  computation. 

Feedforward  Dynamics  Compensation  Scheme  (FED) 

If  the  dynamics  model  of  a  manipulator  is  exact,  then  the  application  of  joint  torques 
computed  from  equation  (I)  by  using  the  reference  trajectory  will  accomplish  trajectory 
tracking.  In  practice,  however,  the  presence  of  modeling  errors  creates  the  need  for  a 
feedback  controller  to  compensate  for  the  small  deviations  in  trajectory  tracking.  The 
feedforward  dynamics  compensation  technique,  depicted  in  Figure  2,  is  based  on  the 
premise  that  the  gross  torque  for  trajectory  tracking  is  provided  by  using  the  inverse 
dynamics  model  in  equation  (1)  in  the  feedforward  path.  This  control  signal  is  then 
augmented  with  the  signal  derived  from  linear  independent  joint  controllers  which  are 
assumed  to  correct  for  small  deviations  in  trajectory  tracking.  The  control  torque  r  is 
therefore: 


r  =  D(«>„  +  R(od,ad)  +  i(ej 

+  J[K,(V«)  + 
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(3) 


where  the  first  three  terms  are  the  feedforward  compensation  torque,  the  last  term  is  the 
torque  due  to  the  feedback  controller,  and  J  is  the  NxN  diagonal  matrix  of  link  inertias  at 
a  typical  position. 

Reduced  Feedforward  Compensation  Scheme  (RFED) 

The  reduced  feedforward  compensation  scheme,  depicted  in  Figure  3,  has  been 
implemented  to  demonstrate  the  effect  of  approximating  the  position  dependent  inertia 
matrix  by  a  constant  diagonal  matrix.  The  control  torque  is  computed  by  substituting  the 
constant  diagonal  inertia  matrix  J  instead  of  D(0^)  in  the  first  term  in  equation  (3).  Thus 
the  torque  applied  to  the  joints  at  each  sampling  instant  is: 

'  =  J[Kp(V*)  +  KA-*)  +  SJ  (0 

+  &(*>„)  +  *(*j) 

where  r  is  the  N  vector  of  applied  control  torques  and  J  is  the  Nx  N  diagonal  matrix  of 
link  inertias  at  a  typical  position. 

The  application  of  the  above  control  laws  is  based  on  the  assumption  that  the  joint  drive 
system  is  a  torque  controlled  device.  To  ensure  that  the  above  assumption  is  satisfied,  we 
have  identified  the  characteristics  of  the  joint  drive  systems.  The  CMU  DD  Arm  D  has 
very  little  friction,  and  is  driven  by  brushless  DC-torque  motors  with  the  amplifiers  which 
control  motor  current  rather  than  voltage  or  speed. 

We  conducted  the  open-loop  small-signal  frequency  response  analysis  and  identified  the 
continuous-time  transfer  function  of  each  joint  Since  our  sampling  period  of  2  ms  is  about 
10  times  smaller  than  the  dominant  mechanical  time  constant  of  the  system,  we  assume 
that  the  effects  of  sampling  are  not  evident  in  the  input-output  response  of  the  system:  this 
assumption  is  indeed  supported  by  the  results  of  the  experiment.  The  identified  transfer 
functions  are  depicted  in  Table  1. 

3.  Controller  Design 

The  performance  of  the  control  schemes  presented  in  the  previous  section  can  be 
compared  only  if  the  same  criteria  are  used  for  design  of  the  controller  gain  matrices. 
Fortunately,  this  is  possible  because  the  gain  matrices  and  Ky  are  the  same  for  all  the 
three  control  schemes  presented  in  this  paper.  The  detailed  procedure  for  choosing  these 
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gain  matrices  and  establishing  an  equivalence  between  the  gains  of  the  nonlinear 
computed- torque  scheme  and  the  linear  independent  joint  control  scheme  is  outlined  by 
Khosla  and  Kanade  [10].  For  expository  convenience,  we  outline  the  criteria  for  the 
selection  of  the  gain  matrices  for  the  computed- torque  scheme.  In  our  experiments,  we 
used  these  gain  values  for  implementing  and  evaluating  the  performance  of  the  RFED  and 
FED  schemes  also. 


3.1.  Design  of  Gain  Matrices  for  Computed-Torque  Scheme 
The  basic  idea  behind  the  computed- torque  scheme  is  to  achieve  dynamic  decoupling  of 
all  the  joints  using  nonlinear  feedback.  If  the  dynamic  model  of  the  manipulator  is 
described  by  equation  (1)  and  the  applied  control  torque  is  computed  according  to  equation 
(2),  then  the  following  closed- loop  system  is  obtained  [15]: 

*  =  u,  -  [0]_1{[D  -  D]0  +  [h  -  fi]  +  [g  -  g]> 

where  u  is  the  commanded  acceleration  signal  and  the  functional  dependencies  on  6  and  9 
have  been  omitted  for  the  sake  of  clarity.  If  the  dynamics  are  modeled  exactly,  that  is, 
D=D,  fT=h  and  g=g,  then  the  decoupled  closed  loop  system  is  described  by 

*  =  U|.  (5) 

The  commanded  acceleration  signal  [10]  is  typically  computed  as: 

-  *„(«„-«)  +  +  »,  (6) 

and  its  substitution  in  equation  (5)  results  in  the  following  closed- loop  characteristic 
equation  for  all  the  joints: 

s2  +  k  4  +  k  =  0  (7) 

vj  pj  v  1 

where  k  .  and  fc are  the  velocity  and  position  gains  for  the  j-th  joint. 

Since  it  is  desired  that  none  of  the  joints  overshoot  the  commanded  position  or  the 
response  be  critically  damped,  our  choice  of  the  matrices  Kp  and  Ky  must  be  ruch  that 
their  elements  satisfy  the  condition: 

kvj  =  for  J  =  l* . ’6  (8) 


Besides,  in  order  to  achieve  a  high  disturbance  rejection  ratio  or  high  stiffness,  it  is  also 
necessary  to  choose  the  position  gain  matrix  K  as  large  as  possible,  which  results  in  a  large 
K 


In  practice,  however,  the  choice  of  the  velocity  gain  Kv  is  limited  by  the  noise  present  m 
the  velocity  measurement.  We  determined  the  upper  limit  of  the  velocity  gain 
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experimentally  we  set  the  position  gain  to  zero  and  increased  the  velocity  gain  of  each 
joint  until  the  unmodeled  high-frequency  dynamics  of  the  system  were  excited  by  the  noise 
introduced  in  the  velocity  measurement  This  value  of  represents  the  maximum 
allowable  velocity  gain  YVe  chose  SOfc  of  the  maximum  velocity  gain  in  order  to  obtain  as 
high  value  of  the  position  gain  as  possible  and  still  be  well  within  the  stability  limits  with 
respect  to  the  unmodeled  high  frequency  dynamics  The  elements  of  the  position  gam 
matrix  were  computed  to  satisfy  the  critical  damping  condition  in  equation  '8:  and 
achieved  the  maximum  disturbance  rejection  ratio  also  The  elements  of  the  velocity  and 
position  gain  matrices  used  in  the  implementation  of  the  control  schemes  are  listed  in 
Table  1 

4.  Experiments  and  Results 

4.1.  Trajectory  Selection  and  Evaluation  Criteria 

Since  the  DD  Arm  II  is  a  highly  nonlinear  and  coupled  system  it  is  impossible  to 
characterize  its  behavior  from  a  particular  class  of  inputs  unlike  linear  systems  for  which  a 
specific  input  such  as  a  unit  step  or  a  ramp  can  be  used  to  design  and  evaluate  the 
controllers  Thus  an  important  constituent  of  the  experimental  evaluation  of  robot  control 
schemes  is  the  choice  of  a  class  of  inputs  for  the  robot  The  criteria  for  selecting  the  joint 
trajectories  is  detailed  by  Khosla  l"8i 

For  evaluating  the  performance  of  robot  control  schemes  we  use  the  dynamic  tracking 
accuracy  This  is  defined  as  the  maximum  position  and  velocity  tracking  error  along  a 
specified  trajectory 

4.2.  Real-Time  Results 

YY  e  have  implemented  the  control  schemes  CT  FED  and  RFED  presented  in  Section  2 
and  evaluated  their  real-time  performance  on  the  six  degrees-of -freedom  <  'Ml  DD  Arm  II 
Because  of  lack  of  space,  we  present  our  results  for  a  simple  but  illustrative  trajectory  used 
to  evaluate  the  above  mentioned  control  schemes 

The  trajectory  is  chosen  to  be  simple  and  relatively  slow  but  capable  of  providing  insight 
into  the  effect  of  dynamics  compensation  In  this  trajectory  only  joints  1  and  2  move 
while  all  the  other  joints  are  commanded  to  hold  their  zero  positions  and  can  be  envisioned 
from  the  schematic  diagram  in  Figure  4  Joint  1  is  commanded  to  start  from  its  zero 
position  and  to  reach  the  position  of  1  5  rad  in  0  75  seconds  it  remains  at  this  position  for 
an  interval  of  0  75  seconds  after  which  it  is  required  to  return  to  its  home  position  in  0  75 
seconds  Similarly  joint  2  is  commanded  to  start  from  its  zero  position  and  to  reach  the 
position  of  -  1  5  rad  in  0  75  seconds  it  remains  at  this  position  for  an  interval  of  0  75 
seconds  after  which  it  is  required  to  return  to  its  home  position  in  0  "5  seconds  The  points 
of  discontinuity  in  the  trajectory  were  joined  by  a  fifth  order  polvnomiai  to  maintain  the 
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continuity  of  position,  velocity  and  acceleration  along  the  three  segments.  The  desired 
position,  velocity  and  acceleration  trajectories  for  joints  1  and  2  are  depicted  in  Figures  5 
and  6,  respectively.  The  absolute  value  of  the  maximum  velocity  and  acceleration  to  be 
attained  by  joints  1  and  2  are  2  rad/sec  and  7.5  rad/sec2,  respectively. 

The  position  and  velocity  tracking  curves  for  the  schemes  CT,  RFED  and  FED  are 
depicted  in  Figures  7  through  10.  The  corresponding  position  and  velocity  tracking  errors 
in  the  three  schemes  for  each  joint  are  shown  in  Figures  11  through  16.  To  give  an  idea  of 
the  relative  performances,  the  maximum  position  and  velocity  tracking  errors  of  each  joint 
are  depicted  in  Table  2.  For  the  sake  of  brevity  we  have  not  included  the  graphs  or 
tabulated  the  values  of  the  errors  of  the  last  three  wrist  joints. 

To  provide  us  with  insight  and  to  aid  in  the  interpretation  of  the  results,  we  outline  the 
dynamic  equations  for  the  first  three  degrees-of-freedom  of  the  CMU  DD  Arm  II: 

rl  ==  ^1 1^1  +  d12*2  +  ^13*3  +  ^133^3 
+  2^123*2*3  +  2^1 13^1^3 +2^112* 

r2  =  d21*l  +  ^22*2  +  rf23*3  +  /l233^3 
2^213*1*3  “  ^211^1 

r3  =  d31*l  +  d32<*2  +  **33*3  ~  ^322^2 
-  2h3]2^jfl2  -  h3ud21  +  g3 

The  coefficients  d  ■,  h. ^  and  g%  are  functions  of  the  joint  position  vector  6  and  are  detailed 
by  Khosla  and  Kanade  [9]. 

The  applied  torque  signals  for  the  three  schemes  are  shown  in  Figures  17—19.  Further, 
decomposition  of  the  applied  torques  in  CT  into  the  inertial,  the  centrifugal  and  Coriolis, 
and  the  gravity  components  of  joints  1  through  3  is  presented  in  Figures  20—22.  We  note 
that  the  applied  torque  for  joint  1  has  a  profile  similar  to  the  desired  acceleration  of  joint  1 
except  during  the  periods  of  constant  speed  (0.75  to  1.0  sec  and  2.25  to  2.5  sec)  in  the 
trajectory  This  suggests  that  the  inertial  torque  dominates  along  most  part  of  the 
trajectory.  This  is  further  supported  by  the  profile  of  the  inertial  torque  component  in 
Figure  20  The  deviations  observed  in  Figure  20  are  due  to  the  Coriolis  and  the  centrifugal 
components  of  the  applied  torque  which  dominate  during  the  period  of  constant  velocity. 

In  the  case  of  joint  2,  the  inertial  component  of  the  applied  torque  curve  in  Figure  21  is 
similar  to  the  profile  of  the  desired  acceleration  of  joint  1  in  Figure  5.  This  implies  that  the 
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applied  torque  of  joint  2  is  dominated  by  the  inertial  coupling  term  ^on£  most  °f  the 
trajectory.  The  term  'n  c<Juati°n  (10)  constitutes  the  centrifugal  torque  and  is  seen 

to  dominate  during  periods  of  constant  velocity. 

Finally,  in  Figure  22  the  inertial  component  of  the  applied  torque  of  joint  3  has  a  profile 
similar  to  the  desired  acceleration  of  joint  2,  when  the  velocity  dependent  torques  are 
negligible.  This  implies  that  the  inertial  coupling  term  dy,09  dominates  along  most  part  of 
the  trajectory.  The  gravitational  torque  in  Figure  22  is  due  to  the  position  errors  and  is 
negligible  compared  to  the  other  torque  components.  It  may  be  seen  from  equation  (11) 
that  the  nonlinear  velocity  dependent  torque  consists  of  both  the  Coriolis  and  the 
centrifugal  components  arising  out  of  the  movement  of  links  1  and  2. 

5.  Conclusions 

The  aim  of  this  paper  has  been  twofold:  first,  to  compare  the  performance  of  the 
feedforward  dynamics  compensation  scheme  with  the  computed- torque  scheme,  and 
second,  to  underscore  the  need  for  including  the  off-diagonal  elements  of  the  inertia  matrix 
in  the  torque  computation.  The  latter  has  been  considered  to  be  important  particularly  in 
the  case  of  direct-drive  arms  where  the  inertial  coupling  effects  are  accentuated  due  to  lack 
of  gears  [2].  It  has  been  demonstrated  in  our  experiments  and  further  supported  by  our 
analysis  that  it  is  possible  for  the  off-diagonal  terms  to  completely  dominate  the  diagonal 
terms  of  the  inertial  matrix  in  the  computation  of  the  joint  actuating  torques.  In  such  an 
event  neglecting  the  off-diagonal  terms  may  lead  to  trajectory  tracking  errors.  It  must  also 
be  pointed  out  that  if  an  exact  model  of  the  manipulator  were  available  then  both  the 
computed- torque  and  feedforward  compensation  scheme  will  give  similar  results.  In  such  a 
circumstance,  using  the  feedforward  dynamics  compensation  may  have  some 
implementational  advantages  because  the  feedforward  torques  could  be  computed  off-line 
and  added  on-line  to  the  torques  computed  by  the  independent  joint  controllers. 


Figure  1:  Block  diagram  of  computed- torque  control  scheme 


Figure  2:  Block  diagram  of  feedforward  compensation  control  scheme 


Figure  3:  Block  diagram  of  independent  joint  control  scheme 


Figure  4:  Schematic  diagram  of  3  DOF  DD  Arm  II 
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Figure  7:  Position  tracking  of  joint  1 
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Figure  8:  Position  tracking  of  joint  2 
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Figure  9:  Velocity  tracking  of  joint  1 
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Figure  13:  Position  tracking  errors  of  joint  3 
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Figure  19:  Applied  torque  of  joint  3 
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Figure  20:  Torque  components  of  joint  1  for  CT 
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Table  3:  Maximum  Tracking  Errors 
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( rads/sec) 

(rads) 
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1 

0.082 
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